/*
 * Copyright (C) 2009 The Android Open Source Project
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

package org.droidfly;

import java.io.IOException;
import java.io.InputStream;
import java.io.OutputStream;
import java.lang.reflect.Method;
import java.security.InvalidParameterException;

import android.app.Service;
import android.bluetooth.BluetoothAdapter;
import android.bluetooth.BluetoothDevice;
import android.bluetooth.BluetoothSocket;
import android.content.Intent;
import android.hardware.Camera.ShutterCallback;
import android.os.Binder;
import android.os.Bundle;
import android.os.Handler;
import android.os.IBinder;
import android.os.Message;
import android.util.Log;

/**
 * This class does all the work for setting up and managing Bluetooth
 * connections with other devices. It has a thread that listens for incoming
 * connections, a thread for connecting with a device, and a thread for
 * performing data transmissions when connected.
 */
public class ServoControlService1 extends Service {
	// Debugging
	private static final String TAG = "ServoControlService";
	private static final boolean D = true;

	// Member fields

	private Handler mHandler;
	private ConnectThread mConnectThread;
	private ConnectedThread mConnectedThread;
	private ReconnectThread mReconnectThread;
	private int mState;

	// Constants that indicate the current connection state
	public static final int STATE_NONE = 0; // we're doing nothing
	public static final int STATE_CONNECTING = 1; // now initiating an outgoing
													// connection
	public static final int STATE_CONNECTED = 2; // now connected to a remote
													// device
	public static final int STATE_RECONNECTING = 3;
	public static final int STATE_SHUTDOWN = 4;

	private final int POSITION_MAX = 254;
	private final int POSITION_MIN = 0;
	private final int STEP = 10;

	private final int SERVO_NUMBER = 6;
	private int[] currentPsitions = null;
	private int[] defaultPsitions = null;
	private int[] commands = null;

	// Binder given to clients
	private final IBinder mBinder = new ServoControlServiceBinder();

	/**
	 * Class used for the client Binder. Because we know this service always
	 * runs in the same process as its clients, we don't need to deal with IPC.
	 */
	public class ServoControlServiceBinder extends Binder {
		ServoControlService1 getService() {
			return ServoControlService1.this;
		}
	}

	@Override
	public IBinder onBind(Intent intent) {
		return mBinder;
	}

	@Override
	public void onCreate() {
		super.onCreate();
		mState = STATE_NONE;
		this.currentPsitions = new int[SERVO_NUMBER];
		this.defaultPsitions = new int[SERVO_NUMBER];
		this.commands = new int[SERVO_NUMBER];
		for (int i = 0; i < SERVO_NUMBER; i++) {
			defaultPsitions[i] = (POSITION_MAX - POSITION_MIN) / 2;
			commands[i] = POSITION_MIN+5;
		}

		if (D)
			Log.d(TAG, "initialized with " + SERVO_NUMBER + " servos");
	}

	@Override
	public void onDestroy() {
		// Stop all threads
		setState(STATE_SHUTDOWN);
		if (D)
			Log.d(TAG, "stop");

		if (mReconnectThread != null) {
			mReconnectThread.cancel();
			mReconnectThread = null;
		}
		if (mConnectThread != null) {
			mConnectThread.cancel();
			mConnectThread = null;
		}
		if (mConnectedThread != null) {
			mConnectedThread.cancel();
			mConnectedThread = null;
		}
	}

	public void setHandler(Handler handler) {
		this.mHandler = handler;
	}

	/**
	 * Set the current state of the chat connection
	 * 
	 * @param state
	 *            An integer defining the current connection state
	 */
	private synchronized void setState(int state) {
		if (D)
			Log.d(TAG, "setState() " + mState + " -> " + state);
		mState = state;

		// Give the new state to the Handler so the UI Activity can update
		mHandler.obtainMessage(DroidFly.MESSAGE_STATE_CHANGE, state, -1)
				.sendToTarget();
	}

	/**
	 * Return the current connection state.
	 */
	public synchronized int getState() {
		return mState;
	}

	/**
	 * Start the ConnectThread to initiate a connection to a remote device.
	 * 
	 * @param device
	 *            The BluetoothDevice to connect
	 */
	public synchronized void connect(BluetoothDevice device) {
		if (D)
			Log.d(TAG, "connect to: " + device);

		if (mState != STATE_RECONNECTING) {
			// Cancel any thread attempting to reconnect
			if (mReconnectThread != null) {
				mReconnectThread.cancel();
				mReconnectThread = null;
			}
		}

		// Cancel any thread attempting to make a connection
		if (mConnectThread != null) {
			mConnectThread.cancel();
			mConnectThread = null;
		}

		// Cancel any thread currently running a connection
		if (mConnectedThread != null) {
			mConnectedThread.cancel();
			mConnectedThread = null;
		}

		// Start the thread to connect with the given device
		mConnectThread = new ConnectThread(device);
		mConnectThread.start();
		setState(STATE_CONNECTING);
	}

	/**
	 * Start the ConnectedThread to begin managing a Bluetooth connection
	 * 
	 * @param socket
	 *            The BluetoothSocket on which the connection was made
	 * @param device
	 *            The BluetoothDevice that has been connected
	 */
	private synchronized void connected(BluetoothSocket socket,
			BluetoothDevice device) {
		if (D)
			Log.d(TAG, "connected");

		// Cancel the thread that completed the connection
		if (mConnectThread != null) {
			mConnectThread.cancel();
			mConnectThread = null;
		}

		// Cancel any thread currently running a connection
		if (mConnectedThread != null) {
			mConnectedThread.cancel();
			mConnectedThread = null;
		}

		// Start the thread to manage the connection and perform transmissions
		mConnectedThread = new ConnectedThread(socket);
		mConnectedThread.start();

		// Send the name of the connected device back to the UI Activity
		Message msg = mHandler.obtainMessage(DroidFly.MESSAGE_DEVICE_NAME);
		Bundle bundle = new Bundle();
		bundle.putString(DroidFly.DEVICE_NAME, device.getName());
		msg.setData(bundle);
		mHandler.sendMessage(msg);
	}

	/**
	 * Indicate that the connection attempt failed and notify the UI Activity.
	 */
	private void connectionFailed() {
		setState(STATE_NONE);

		// Send a failure message back to the Activity
		Message msg = mHandler.obtainMessage(DroidFly.MESSAGE_TOAST);
		Bundle bundle = new Bundle();
		bundle.putString(DroidFly.TOAST, "Unable to connect");
		msg.setData(bundle);
		mHandler.sendMessage(msg);
	}

	/**
	 * Indicate that the connection was lost and notify the UI Activity.
	 */
	private void connectionLost(BluetoothDevice device) {
		if (mState == STATE_SHUTDOWN)
			return;

		setState(STATE_RECONNECTING);
		mReconnectThread = new ReconnectThread(device);
		mReconnectThread.start();

		// Send a failure message back to the Activity
		Message msg = mHandler.obtainMessage(DroidFly.MESSAGE_TOAST);
		Bundle bundle = new Bundle();
		bundle.putString(DroidFly.TOAST, "Device connection was lost");
		msg.setData(bundle);
		mHandler.sendMessage(msg);
	}

	/**
	 * This thread runs while attempting to make an outgoing connection with a
	 * device. It runs straight through; the connection either succeeds or
	 * fails.
	 */
	private class ConnectThread extends Thread {
		private final BluetoothSocket mmSocket;
		private final BluetoothDevice mmDevice;

		public ConnectThread(BluetoothDevice device) {
			mmDevice = device;
			BluetoothSocket tmp = null;
			setName("ConnectThread");

			// Get a BluetoothSocket for a connection with the
			// given BluetoothDevice
			try {
				Method m = device.getClass().getMethod("createRfcommSocket",
						new Class[] { int.class });
				tmp = (BluetoothSocket) m.invoke(device, 1);
			} catch (Exception e) {
				Log.e(TAG, "create() failed", e);
			}
			mmSocket = tmp;
		}

		@Override
		public void run() {
			Log.i(TAG, "BEGIN mConnectThread");

			// Always cancel discovery because it will slow down a connection
			BluetoothAdapter adapter = BluetoothAdapter.getDefaultAdapter();
			adapter.cancelDiscovery();

			if (mmSocket == null) {
				connectionFailed();
				return;
			}

			// Make a connection to the BluetoothSocket
			try {
				// This is a blocking call and will only return on a
				// successful connection or an exception
				mmSocket.connect();
			} catch (IOException e) {
				connectionFailed();
				// Close the socket
				try {
					mmSocket.close();
				} catch (IOException e2) {
					Log.e(TAG,
							"unable to close() socket during connection failure",
							e2);
				}
				return;
			}

			// Reset the ConnectThread because we're done
			synchronized (ServoControlService1.this) {
				mConnectThread = null;
			}

			// Start the connected thread
			connected(mmSocket, mmDevice);
		}

		public void cancel() {
			try {
				mmSocket.close();
			} catch (IOException e) {
				Log.e(TAG, "close() of connect socket failed", e);
			}
		}
	}

	/**
	 * This thread runs during a connection with a remote device. It handles all
	 * incoming and outgoing transmissions.
	 */
	private class ConnectedThread extends Thread {
		private final BluetoothSocket mmSocket;
		private final BluetoothDevice mmDevice;
		private final InputStream mmInStream;
		private final OutputStream mmOutStream;

		public ConnectedThread(BluetoothSocket socket) {
			Log.d(TAG, "create ConnectedThread");
			mmSocket = socket;
			mmDevice = socket.getRemoteDevice();
			InputStream tmpIn = null;
			OutputStream tmpOut = null;
			setName("ConnectedThread");

			// Get the BluetoothSocket input and output streams
			try {
				tmpIn = socket.getInputStream();
				tmpOut = socket.getOutputStream();
			} catch (IOException e) {
				Log.e(TAG, "temp sockets not created", e);
			}

			mmInStream = tmpIn;
			mmOutStream = tmpOut;
		}

		@Override
		public void run() {
			Log.i(TAG, "BEGIN mConnectedThread");

			if (mmInStream == null || mmOutStream == null) {
				connectionFailed();
				return;
			}

			setState(STATE_CONNECTED);			
			// Keep listening to the InputStream while connected
			while (true) {				

				try {
					for (int servo = 0; servo < commands.length; servo++) {
						int position = -1;
						synchronized (commands) {
							position = commands[servo];
							if (position >= 0)
								commands[servo] = -1;
						}
						if (position >= 0) {
							byte[] command = createCommand(servo, position);
							mmOutStream.write(command);							
						}
					}
				} catch (IOException e) {					
					Log.e(TAG, "Exception during write", e);
					connectionLost(mmDevice);
					break;
				}
			}
		}
		
		public void cancel() {
			try {
				mmSocket.close();
			} catch (IOException e) {
				Log.e(TAG, "close() of connect socket failed", e);
			}
		}
	}

	/**
	 * This thread runs after connection was lost. It tries to reconnect to the
	 * last connected BT device.
	 */
	private class ReconnectThread extends Thread {
		private BluetoothDevice mmDevice;

		public ReconnectThread(BluetoothDevice device) {
			Log.d(TAG, "connection was lost, reconnect");
			mmDevice = device;
			setName("ReconnectThread");
		}

		@Override
		public void run() {
			while (true) {
				if (mmDevice == null)
					return;

				switch (mState) {
				case STATE_CONNECTED:
					// Reset the ReconnectThread because we're done
					synchronized (ServoControlService1.this) {
						mReconnectThread = null;
					}
					return;
				case STATE_CONNECTING:
					continue;					
				default:
					setState(STATE_RECONNECTING);
					connect(mmDevice);
				}
			}
		}

		public void cancel() {
			mmDevice = null;
			setState(STATE_NONE);
		}
	}

	// //////// SERVO CONTROL /////////////////////

	public int getMinPosition() {
		return this.POSITION_MIN;
	}

	public int getMaxPosition() {
		return this.POSITION_MAX;
	}

	public void moveServoUp(int servoNumber) throws IOException {
		if (currentPsitions[servoNumber] >= this.POSITION_MAX)
			return;

		int newPosition = currentPsitions[servoNumber] + STEP;
		if (newPosition > this.POSITION_MAX)
			newPosition = this.POSITION_MAX;

		this.moveServoToPosition(servoNumber, newPosition);
	}

	public void moveServoDown(int servoNumber) throws IOException {
		if (currentPsitions[servoNumber] <= this.POSITION_MIN)
			return;

		int newPosition = currentPsitions[servoNumber] - STEP;
		if (newPosition < this.POSITION_MIN)
			newPosition = this.POSITION_MIN;

		this.moveServoToPosition(servoNumber, newPosition);
	}

	public void moveServoToPosition(int servo, int position) throws IOException {
		if (position > this.POSITION_MAX)
			position = this.POSITION_MAX;

		if (position < this.POSITION_MIN)
			position = this.POSITION_MIN;

		synchronized (this) {
			commands[servo] = position;
		}
	}

	private byte[] createCommand(int servo, int position) {
		if (position > 255 || position < 0)
			throw new InvalidParameterException("Wrong position " + position);

		byte[] bytes = new byte[3];
		bytes[0] = (byte) 0xFF;
		bytes[1] = (byte) servo;
		bytes[2] = (byte) position;

		return bytes;
	}
}
